#include <eigen3/Eigen/Dense>
#include "OsqpEigen/OsqpEigen.h"    // osqp-eigen
#include <iostream>

#include "matplotlibcpp.h"

namespace plt = matplotlibcpp;

#include <chrono>

void H_f(unsigned int N, Eigen::VectorXd &init_x, Eigen::MatrixXd &A, Eigen::MatrixXd &B,Eigen::MatrixXd &Cy,
                       Eigen::MatrixXd &Q, Eigen::MatrixXd &R, Eigen::MatrixXd &F,  Eigen::MatrixXd &Q_bar, Eigen::MatrixXd &R_bar,
                        Eigen::MatrixXd &Fy,Eigen::MatrixXd &Gy) {
    unsigned int n = init_x.rows();
    unsigned int m = B.cols();
    unsigned int r = Cy.rows();

    Eigen::MatrixXd C, M;
    // Gy.resize((N + 1) * r, m * N);
    // Gy.setZero();
    // Fy.resize((N + 1) * r, n);

    Eigen::MatrixXd temp;
    temp.resize(n, n);
    temp.setIdentity();//和A矩阵一样的n*n 对角单位阵 因为要和A乘
        Eigen::MatrixXd I;
    I.resize(n, n);
    I.setIdentity();//和A矩阵一样的n*n 对角单位阵 因为要和A乘
    Fy.block(0, 0, r, n) = Cy * I;//block(i, j, rows, cols) 是 Eigen 提供的函数，用于提取矩阵的一个子块（block）。
                                        //这里 (0, 0, n, n) 表示从 (0, 0)（左上角）开始，提取一个 n × n 的子矩阵。
                                        //这是M矩阵的第一个大行 In*n
    // for (unsigned int i = 1; i <= N; ++i) {//填充M C矩阵的剩下N大行

    //     Eigen::MatrixXd temp_gy;
    //     temp_gy.resize(r, (N+1) * m);
    //     temp_gy << Cy * temp * B, Gy.block(r * (i - 1), 0, r, Gy.cols());//<< 表示 按顺序填充数据，类似于 MATLAB 或 Python 中的 [A; B] 垂直拼接。
    //                                                             //temp * B 是一个矩阵（或向量）运算结果，它会作为 temp_c 的 第一部分。
    //                                                             //C.block(n * (i - 1), 0, n, C.cols()) 是 C 的一个子矩阵，它会作为 temp_c 的 第二部分，拼接 在 temp*B 右面。
    //                                                             //C.block(n * (i - 1), 0, n, C.cols()) 是上一次循环的全部n*m列的C矩阵 再和temp * B拼接 就多出了一个m列 所以temp_c要(N + 1) * m列
    //                                                             //C的第n*(i-1)行到n*i行 i=0即C的第1个大行 即全是0的大行
    //     Gy.block(r * i, 0, r, Gy.cols())
    //             = temp_gy.block(0, 0, r, temp_gy.cols() - m);
    //     temp = A * temp;
    //     // Fy 填第 i 块
    //     Fy.block(i * r, 0, r, n) = Cy * temp;
    // }
    for (unsigned int i = 1; i <= N; ++i) {
        // 构建 temp_gy，拼接一行：Cy * temp * B + 前一行的 Gy
        Eigen::MatrixXd temp_gy(r, N * m); // ⛔ 原来这里多给了 m 列，其实只需要 N*m 列就够
        temp_gy.setZero();
        
        temp_gy.block(0, 0, r, m) = Cy * temp * B;
        temp_gy.block(0, m, r, Gy.cols() - m) = Gy.block(r * (i - 1), 0, r, Gy.cols()-m);

        // 写入 Gy 的第 i 大行
        Gy.block(r * i, 0, r, Gy.cols()) = temp_gy.block(0, 0, r, Gy.cols());

        // 更新 temp 和 Fy
        temp = A * temp;
        Fy.block(i * r, 0, r, n) = Cy * temp;
    }

    // Eigen::MatrixXd Q_bar, R_bar;

    Q_bar.resize(r * (N + 1), r * (N + 1));
    Q_bar.setZero();
    for (unsigned int i = 0; i < N; ++i) {//0大行到N-1大行 共N大行 共N*n行
        Q_bar.block(r * i, r * i, r, r) = Q;
    }
    Q_bar.block(r * N, r * N, r, r) = F;//第N+1大行

    R_bar.resize(N * m, N * m);
    R_bar.setZero();
    for (unsigned int i = 0; i < N; ++i) {
        R_bar.block(i * m, i * m, m, m) = R;
    }

    // Eigen::MatrixXd G = M.transpose() * Q_bar * M;
    // Eigen::MatrixXd E = C.transpose() * Q_bar * M;
    // Eigen::MatrixXd H = C.transpose() * Q_bar * C + R_bar;

    // Eigen::MatrixXd G = M.transpose() * Q_bar * M;
    // f = 2*E.transpose() * Q_bar * Gy;
    // H = Gy.transpose() * Q_bar * Gy + R_bar;
}
Eigen::VectorXd RunMPC(Eigen::VectorXd &init_x, Eigen::MatrixXd &f, Eigen::MatrixXd &H, Eigen::MatrixXd &E,
                         Eigen::MatrixXd &Q_bar, Eigen::MatrixXd &R_bar,Eigen::MatrixXd &Gy) {
    
    // f = 2*E.transpose() * Q_bar * Gy;
    f = 2*Gy.transpose() * Q_bar * E;
    H = Gy.transpose() * Q_bar * Gy + R_bar;
    std::cout<<"////////////"<<std::endl;
    std::cout<<"H的逆是 "<<H.inverse().rows()<<" "<<H.inverse().cols()<<std::endl;
    std::cout<<"f "<<f.rows()<<" "<<f.cols()<<std::endl;
    std::cout<<"init_x "<<init_x.rows()<<" "<<init_x.cols()<<std::endl;
    // return 2*H.inverse() * (-2* f* init_x);
    // return -1*2*H.inverse()*f;
    Eigen::VectorXd U = -H.ldlt().solve(f);  // 使用 LDLT 分解求解线性系统
    std::cout<<"////////////"<<std::endl;
    return U;
}
Eigen::VectorXd RunMPC_OSQP(Eigen::VectorXd &init_x, Eigen::MatrixXd &f, Eigen::MatrixXd &H, Eigen::MatrixXd &E,
                         Eigen::MatrixXd &Q_bar, Eigen::MatrixXd &R_bar,Eigen::MatrixXd &Gy,int &N,int &m,int &n
                        ,OsqpEigen::Solver& solver) {
    
//     f = 2*Gy.transpose() * Q_bar * E;// 得到 1 x (N*m) 行向量
//     Eigen::VectorXd f_T = f.transpose(); // 转为 (N*m)x1 的列向量，符合 OSQP 要求
//     H = 2*(Gy.transpose() * Q_bar * Gy + R_bar);

//     std::cout<<"////////////"<<std::endl;
//     std::cout<<"H的逆是 "<<H.inverse().rows()<<" "<<H.inverse().cols()<<std::endl;
//     std::cout<<"f是 "<<f.rows()<<" "<<f.cols()<<std::endl;
//     // 创建求解器：
//     // 注意：这句话只是实例化了一个求解器对象，但并未对其进行任何的配置
//     OsqpEigen::Solver solver;
//     // 配置求解器的设置：
//     solver.settings()->setVerbosity(false);
//     // 解释：这行代码会设置求解器的输出冗长程度，setVerbosity(false) 会关闭求解器的详细输出，使其在求解过程中不输出额外的信息，这在需要安静地运行求解器时非常有用；
//     solver.settings()->setWarmStart(true);
//     // 解释：启用 WarmStart 功能，加快求解速度（启用 WarmStart 意味着求解器在求解问题时，可以利用之前求解的结果作为初始猜测来加速收敛，这在处理连续求解类似问题时特别有用，可以显著减少计算时间）；

//     // 配置求解器的数据：
//     // 包括：变量数目 + 约束数目 + 矩阵P + 梯度向量q + 线性约束矩阵A_c + 变量下界l + 上界u
//     // 解释：如果传入失败，各个函数返回值为 false
//     Eigen::VectorXd U_K;
//     U_K.resize(N*m);
//     int num_vars = U_K.rows();
//     int num_constraints = N * m;  // 每个变量一个上下界
//     Eigen::SparseMatrix<double> H_sparse = H.sparseView();//将 稠密矩阵（Dense Matrix） H 转换为 稀疏矩阵（Sparse Matrix） H_sparse，以便 OSQP 高效处理。
//     solver.data()->setNumberOfVariables(num_vars);          // 设置优化问题的变量数目（ x_0 ~ x_N  +  u_0 ~ u_N-1 ）
//     solver.data()->setNumberOfConstraints(num_constraints);    // 设置优化问题的约束数目
//     solver.data()->setHessianMatrix(H_sparse);                            // 传入 P 矩阵
//     solver.data()->setGradient(f_T);                           // 传入 q 向量
//     // 无约束，所以 A 是空的
//     // Eigen::SparseMatrix<double> A_sparse(2*n*(N+1)+N*m, (N+1)*n+N*m);
//     Eigen::SparseMatrix<double> A_sparse(num_constraints,num_vars);
//     solver.data()->setLinearConstraintsMatrix(A_sparse);// 传入 A_c 矩阵
//     Eigen::VectorXd lower_bound(num_constraints), upper_bound(num_constraints);
//     lower_bound.setConstant(-1e10); // 或者根据变量物理限制设定
//     upper_bound.setConstant(1e10);
//     solver.data()->setLowerBound(lower_bound);// 传入 l
//     solver.data()->setUpperBound(upper_bound); // 传入 u                       

//     // 初始化求解器
//     if (!solver.initSolver()) {
//         std::cerr << "OSQP initialization failed!" << std::endl;
//         return Eigen::VectorXd::Zero(num_vars);
//     }

//     // 求解
//     if (!solver.solve()) {
//         std::cerr << "OSQP solve failed!" << std::endl;
//         return Eigen::VectorXd::Zero(num_vars);
//     }
//  std::cout<<"////////////*****/////"<<std::endl;
//     U_K = solver.getSolution();//// 将 QP 问题的解存储在向量control_sequence
//     std::cout<<"Run_Mpc_OSQP中求解完成///"<<std::endl;
//     return U_K;


    // 2. 计算Hessian和梯度
    H = 2 * (Gy.transpose() * Q_bar * Gy + R_bar);
    Eigen::MatrixXd E_T = E.transpose();
    f = 2 * E_T * Q_bar * Gy;


    // // 检查矩阵正定性
    // Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigensolver(H);
    // if (eigensolver.eigenvalues().minCoeff() <= 1e-6) {
    //     std::cerr << "Warning: H matrix is not positive definite!" << std::endl;
    //     H += 1e-6 * Eigen::MatrixXd::Identity(H.rows(), H.cols());
    // }

    // 3. 转换为OSQP格式
    Eigen::VectorXd f_vec = f.transpose();
    Eigen::SparseMatrix<double> H_sparse = H.sparseView();

    // 4. 设置OSQP求解器
    // OsqpEigen::Solver solver;
    solver.settings()->setVerbosity(false);  // 调试时开启
    solver.settings()->setWarmStart(true);
    // solver.settings()->setMaxIteration(4000);  // 增加迭代次数

    int num_vars = N * m;
    int num_constraints = num_vars;

    // 5. 设置约束矩阵（单位矩阵）
    Eigen::SparseMatrix<double> A_sparse(num_constraints, num_vars);
    A_sparse.setIdentity();

    // 6. 设置合理的控制输入约束
    Eigen::VectorXd lower_bound(num_constraints), upper_bound(num_constraints);
    lower_bound.setConstant(-55);  // 根据系统物理限制调整
    upper_bound.setConstant(55);

    // 7. 配置求解器（带错误检查）
    solver.data()->setNumberOfVariables(num_vars);
    solver.data()->setNumberOfConstraints(num_constraints);
    if (!solver.data()->setHessianMatrix(H_sparse)) {
        std::cerr << "Failed to set Hessian matrix" << std::endl;
        return Eigen::VectorXd::Zero(num_vars);
    }
    if (!solver.data()->setGradient(f_vec)) {
        std::cerr << "Failed to set gradient vector" << std::endl;
        return Eigen::VectorXd::Zero(num_vars);
    }
    if (!solver.data()->setLinearConstraintsMatrix(A_sparse)) {
        std::cerr << "Failed to set constraint matrix" << std::endl;
        return Eigen::VectorXd::Zero(num_vars);
    }
    if (!solver.data()->setLowerBound(lower_bound)) {
        std::cerr << "Failed to set lower bound" << std::endl;
        return Eigen::VectorXd::Zero(num_vars);
    }
    if (!solver.data()->setUpperBound(upper_bound)) {
        std::cerr << "Failed to set upper bound" << std::endl;
        return Eigen::VectorXd::Zero(num_vars);
    }

    // 8. 初始化并求解
    if (!solver.initSolver()) {
        std::cerr << "OSQP initialization failed!" << std::endl;
        return Eigen::VectorXd::Zero(num_vars);
    }

    if (!solver.solve()) {
        std::cerr << "OSQP solve failed!" << std::endl;
        return Eigen::VectorXd::Zero(num_vars);
    }

    // 9. 获取解并确保维度正确
    Eigen::VectorXd U_K = solver.getSolution();
    if (U_K.size() != N*m) {
        std::cerr << "Warning: Solution size mismatch! Expected " << N*m 
                 << ", got " << U_K.size() << std::endl;
        return Eigen::VectorXd::Zero(N*m);
    }
    std::cout<<"///////////"<<std::endl;
    std::cout << "Gy ptr: " << Gy.data() << std::endl;
std::cout << "f ptr: " << f.data() << std::endl;
std::cout << "E ptr: " << E.data() << std::endl;

    return U_K;

    // std::cout<<"////////////"<<std::endl;
    // std::cout<<"H的逆是 "<<H.inverse().rows()<<" "<<H.inverse().cols()<<std::endl;
    // std::cout<<"f是 "<<f.rows()<<" "<<f.cols()<<std::endl;
    // std::cout<<"init_x "<<init_x.rows()<<" "<<init_x.cols()<<std::endl;
    // // return 2*H.inverse() * (-2* f* init_x);
    // return -1*2*H.inverse()*f;
}
int main() {
    // const unsigned int N = 2;//定义预测区间N步
    // const unsigned int K_steps = 200;//定义预测区间N步
    int N = 20;//定义预测区间N步
    int K_steps = 200;//定义预测区间N步
    Eigen::MatrixXd A, B, Cy, Fy, Gy;
    A.resize(2, 2);
    B.resize(2, 1);
    Cy.resize(2, 2);

    A << 1, 0.1, 0,0.9;
    B << 0, 0.5;
    Cy << 1, 0, 0, 1;
    int n = A.rows();
    int m = B.cols();
    int r = Cy.rows();
    
    Eigen::MatrixXd Q, R, F;
    Q.resize(r, r);//对角线上有数
    Q << 10, 0, 0, 10;

    R.resize(m, m);
    R << 0.1;

    F.resize(r, r);
    F << 2, 0, 0, 2;

    Fy.resize((N + 1) * r, n);
    Fy.setZero();
    Gy.resize((N + 1) * r, N*m);
    Gy.setZero();

    // Eigen::VectorXd init_x;
    // init_x.resize(n, 1);
    // init_x << 5.0, 5.0;
    Eigen::MatrixXd U;
    U.resize(m,K_steps);
    U.setZero();

    Eigen::MatrixXd X;
    X.resize(n,K_steps);//这里不是定义中间预测的变量的值 而是定义离散的状态空间方程中的x y u
    X.setZero();
    // Eigen::VectorXd init_x = X.col(0);
    // init_x << 1, 1; 
    Eigen::VectorXd init_x(n);
    init_x << 10, 10;
    X.block(0, 0, n, 1) = init_x;

    Eigen::MatrixXd Y;
    Y.resize(r,K_steps);

    Eigen::MatrixXd f;
    Eigen::MatrixXd H;
    Eigen::MatrixXd Q_bar, R_bar;
    // Eigen::MatrixXd state_0;
    Eigen::VectorXd state_1;
    std::vector<double> state_0;
    std::vector<double> time;
    state_0.emplace_back(init_x.x());//init_x.x()获取第一个元素，等价于 init_x[0]
    time.emplace_back(0.0);
    // Gy.resize((N + 1) * r, m * N);
    // Gy.setZero();
    // Fy.resize((N + 1) * r, n);
    Eigen::MatrixXd R_ref;
    R_ref.resize((N+1)*r,1);
    Eigen::MatrixXd E;
    // Eigen::MatrixXd E;
    // E.resize((N+1)*r,1);
    std::cout<<"R_ref是 "<<R_ref.rows()<<" "<<R_ref.cols()<<std::endl;
std::unique_ptr<OsqpEigen::Solver> solver(new OsqpEigen::Solver());
    for (unsigned int i = 0; i < K_steps-1; ++i) {//总共是200步
        H_f(N, init_x, A, B, Cy,Q, R, F,Q_bar,R_bar,Fy,Gy);//求得  使得代价/目标函数J最小 的U_K(:,k)即control   也就是每一步都算出一个新的系统输入
                
        Eigen::MatrixXd r_error;
        r_error.resize(r,1);
        for(int j=0; j<=N; j++)
        {
            // int r_error_1 = 20 * cos(0.05 * i);
            // int r_error_2 = 20 * sin(0.05 * i);
            int r_error_1 = 50;
            int r_error_2 = 50;
            r_error << r_error_1,r_error_2;
            R_ref.block(j*r,0,r,1) = r_error;
        }
        E = Fy*X.block(0,i, n,1)-R_ref;
        std::cout << "error: " << init_x.transpose() << std::endl;
        std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now();
        // Eigen::VectorXd control = RunMPC(init_x, f,H);//求得  使得代价/目标函数J最小 的U_K(:,k)即control   也就是每一步都算出一个新的系统输入
        std::chrono::steady_clock::time_point end_time = std::chrono::steady_clock::now();
        std::chrono::duration<double> used_time = (end_time - start_time);
        std::cout << "Once mpc use time(ms): " << used_time.count() * 1000 << std::endl;

        // Eigen::MatrixXd E;
        // E.resize((N+1)*r,1);
        // Eigen::MatrixXd E = Fy*X.block(i*n,0, n,n)-R_ref;

        // Eigen::VectorXd U_K = RunMPC(init_x, f,H,E,Q_bar,R_bar,Gy);//求得  使得代价/目标函数J最小 的U_K(:,k)即control   也就是每一步都算出一个新的系统输入
        // 使用 OSQP 求解 MPC   U_K  是 (N * m) × 1
        Eigen::VectorXd U_K = RunMPC_OSQP(init_x, f,H,E,Q_bar,R_bar,Gy,N,m,n,*solver);
        std::cout<<"OSQP求解U_K完成"<<std::endl;
        // 取第一个控制输入
        Eigen::VectorXd U = U_K.head(m);//U_K.head(m);返回的是 前 m 个元素组成的子向量（仍然是一个 m × 1 的列向量）
        std::cout<<"取U_K前m行"<<std::endl;
        // U.block(0, i, m, 1) = control;
        std::cout<<"///////***/////"<<std::endl;
        X.block(0,i+1, n,1) = A * X.block(0,i, n,1) + B * U;//%计算k+1步的状态变量的值  取control的第一列
        // state_0.emplace_back(i);
        state_1 = X.block(0,i, n,1);
        state_0.emplace_back(state_1.x());  
        time.emplace_back(0.1 * (i + 1));

    }

    plt::plot(time, state_0, "ro");
    plt::xlim(-0.0, 20.0);
    plt::ylim(-0.0, 70.0);
    plt::title("MPC");
    plt::show();

//    std::cout << "closed form u: " << control.transpose() << std::endl;

    return 0;
}
